#!/usr/bin/perl

%MOTOR_PATH;  # Motor Mapping  "A" => "Dirver Path"
%SENSOR_PATH;  # SENSOR Mapping  "1" => "Dirver Path"
%SENSOR_NAME;  # SESON PATH.

# 初始化
getMotorInfo();
getSensorInfo();

# Get Motor Info
sub getMotorInfo{
	for($i=0;$i<5;$i++){
		my $add="/sys/class/tacho-motor/motor$i/address";
		if(-e $add){
			my $content=`cat $add`;
			(my $port)=$content=~/out((A|B|C|D))/;
			$MOTOR_PATH{$port}="/sys/class/tacho-motor/motor$i";
		}
	}
}

# Get Sensor Info
sub getSensorInfo{
	my $SENSOR_DIR="/sys/class/lego-sensor";
	opendir (DIR, $SENSOR_DIR) || die "Error in opening dir $SENSOR_DIR\n";
	while(($filename = readdir(DIR))){
		next if($filename!~/sensor/);  # 非sensor 开头的文件忽略
		chop (my $address=`cat $SENSOR_DIR/$filename/address`);
		chop (my $sensorname=`cat $SENSOR_DIR/$filename/driver_name`);
		$SENSOR_NAME{$address}=$sensorname;
		$SENSOR_PATH{$address}="$SENSOR_DIR/$filename";
	}
	closedir(DIR);
}

#  $1  制定时间 $2:指定速度负数为倒桩。 $3 $4
# runtimed(1000,0.1,"B","C"); # 运行1秒，0.1马力，先前，驱动B和C马达
# runtimed();  #使用默认值: 运行2秒，0.8马力，先前，驱动B和C马达
# runtimed(1500,-0.8);  # 以0.8倍马力，向后运行1.5秒

sub runtimed{
	$time=shift or $time=2000;  # 默认 2 seconds
	$power=shift or $power=0.8;
	$driverB=shift or $driverB ="B";
	$driverC=shift or $driverC ="C";
	
	# print " $time $driverB  $driverC $power\n";
	$speedb="$MOTOR_PATH{$driverB}/speed_sp";
	$speedc="$MOTOR_PATH{$driverC}/speed_sp";
	$timeb="$MOTOR_PATH{$driverB}/time_sp";
	$timec="$MOTOR_PATH{$driverC}/time_sp";
	
	$maxsp=`cat $MOTOR_PATH{$driverB}/max_speed`;
	$spspeed=$maxsp*$power;
	
	`echo $time | tee $timeb $timec`;
	`echo $spspeed |tee $speedb $speedc`;
	`echo run-forever | tee $MOTOR_PATH{$driverC}/command $MOTOR_PATH{$driverB}/command`;
	sleep $time/1000;
	`echo stop | tee $MOTOR_PATH{$driverC}/command $MOTOR_PATH{$driverB}/command`;
}


#  $1 制定的圈数
# runcycle(3,0.1,"B","C"); # 运行1圈，0.1马力，先前，驱动B和C马达
# runcycle();  #使用默认值: 运行2圈，0.8马力，先前，驱动B和C马达
# runcycle(1500,-0.8);  # 以0.8倍马力，向后运行1.5圈

sub runcycle{
	$time=shift or $time=2000;  # 默认 2 seconds
	$power=shift or $power=0.8;
	$driverB=shift or $driverB ="B";
	$driverC=shift or $driverC ="C";
	
	# print " $time $driverB  $driverC $power\n";
	$speedb="$MOTOR_PATH{$driverB}/speed_sp";
	$speedc="$MOTOR_PATH{$driverC}/speed_sp";
	$timeb="$MOTOR_PATH{$driverB}/time_sp";
	$timec="$MOTOR_PATH{$driverC}/time_sp";
	
	$maxsp=`cat $MOTOR_PATH{$driverB}/max_speed`;
	$spspeed=$maxsp*$power;
	
	`echo $time | tee $timeb $timec`;
	`echo $spspeed |tee $speedb $speedc`;
	`echo run-forever | tee $MOTOR_PATH{$driverC}/command $MOTOR_PATH{$driverB}/command`;
	sleep $time/1000;
	`echo stop | tee $MOTOR_PATH{$driverC}/command $MOTOR_PATH{$driverB}/command`;
}
